% C \in S^{n(p+1)}
% Minimizes E(||Omega^(1/2)*w(t)||_2) - estimation error normalized

function [B,converged] = regular_rgr(C, n, p, gamma, tol, max_iter)
	converged = false;
	Omega = inv(C(1:n,1:n));
    ntest = n;
	rel_err = 1;
	itercount = 0;

    while(rel_err>tol && itercount<max_iter)
        
    % Estimate A given Omega
        cvx_begin
            variable X(n*(p+1),n*(p+1)) symmetric
        % construct D
            expression D(n, n*(p+1));
            for i=0:p
                D(1:n,1:n) = D(1:n,1:n) + X((1:n)+n*i,(1:n)+n*i);
            end
            for k=1:p
                for i=0:(p-k)
                    D(1:n,(1:n)+n*k) = D(1:n,(1:n)+n*k) + 2*X((1:n)+n*i,(1:n)+n*(i+k));
                end
            end
        % evaluate penalty h
            expression h(1);
            for i=1:n
                for j=(i+1):n
                    h = h + norm([D(i,j+n*(0:p)) D(j,i+n*(0:p))], Inf);
                end
            end
            minimize(trace(C*X) + gamma*h);
            subject to 
                X(1:n,1:n) == min(Omega,Omega');
                X == semidefinite(n*(p+1));
        cvx_end

        B0 = sqrtm(X(1:n,1:n));
        B1p = B0 \ X(1:n,(1+n):(n*(p+1)));
        A = [eye(n) B0\B1p];
    
    % Estimate Omega given A
        Omega_new = (A*C*A')\eye(n);
        ntest = [ntest norm(Omega-Omega_new,'fro')];
        Omega = Omega_new;

		rel_err = ntest(end)/norm(Omega,'fro');
		fprintf('CVX optimal %d, relative error ||Omega_new-Omega||_F/||Omega_new||_F %d\n',cvx_optval,rel_err);
    	itercount = itercount +1;
    end
		if itercount < max_iter, converged = true; end
    B = [B0 B1p];
end
